clear;
initialize;
subscribeKinect;

pose = [0 0 0];
pas = 10;


[image, depthMap, timestamp] = ros4mat('kinect');
world = depthTo2DCloud(depthMap);
%world(isnan(world)) = [];
% subplot(1,3,1)
% plot(world(:,1), world(:,2), 'r.'); axis equal
for a = [0:pas:40]
    roombaTurnAngle(0.03, pas);
    pause(5);
    angle = getRotationAngle;
    %pose(3) = pose(3) + angle
    pose(3) = pose(3) + (pas * pi / 180);

    [image, depthMap, timestamp] = ros4mat('kinect');
    new_points = depthTo2DCloud(depthMap);
    % subplot(1,3,2)
    % plot(new_points(:,1), new_points(:,2), 'r.'); axis equal
    [world, pose] = getCurrentLocationAndUpdate(world, new_points(:,1:2), pose);
    % subplot(1,3,3)
    % plot(world(:,1), world(:,2), 'r.'); axis equal;
    pose
end
plot(world(:,1), world(:,2), 'r.'); axis equal;
finalize;